/*
* 电机指令列表
 * 'o' 利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息
 * 'v' 利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。
 *     例：v1024 - 将电机moveTo到1024位置
 * 'm' 利用move函数使电机运行相应步数。move函数不会“block”程序。
 *     例：m1024 - 使电机运行1024步 
 * 'r' 利用runToNewPosition函数让电机运行到用户指定坐标位置。runToNewPosition函数功能
 *     与moveTo函数功能基本相同。唯一区别是runToNewPosition函数将“block”程序运行。
 *     即电机没有到达目标位置前，Arduino将不会继续执行后续程序内容。
 *     这一点很像Arduino官方Stepper库中的step函数。
 *     例：r1024 - 将电机runToNewPosition到1024位置。Arduino在电机达到1024位置前将停止
 *     其它程序任务的执行。
 * 's' 利用setCurrentPosition函数设置当前位置为用户指定位置值。
 *     例：s0 - 设置当前位置为0
 * 'a' 利用setAcceleration函数设置加速度 
 *     例：a100 - 设置电机运行的加速度为100 
 * 'x' 利用setMaxSpeed函数设置最大速度 
 *     例：x500 - 设置电机运行的最大速度为500 
 * 'd' 用户通过此指令可指定哪一个电机进行工作  
 *     例：d1 一号电机工作，d2 二号电机工作, d3 三号电机工作,d4 四号电机工作,d0 4个电机同时工作
 /////////////////////舵机///////////////////
 * 'h   ' 将控制舵机one
 *     例：one45 - 设置舵机旋转到45度
 * 'j' 将控制舵机two
 *     例：two45 - 设置舵机旋转到45度
 * 'k' 将控制舵机three
 *     例：three45 - 设置舵机旋转到45度
 * 'l' 将控制舵机four
 *     例：four45 - 设置舵机旋转到45度
 * 'p' 用户通过此指令舵机状态  
 * 'i' 用户通过此指令将舵机调为90度
 * 'e' 使能电机驱动板
 * 'n' 取消使能电机驱动板
*/
#include <AccelStepper.h>  //使用AccelStepper库
#include <Servo.h>         //使用servo库

char cmd;        //电机指令字符
int data;        //电机指令参数
int stepperNum;  //控制电机编号

int DSD = 15;  //Default Servo Delay (默认电机运动延迟时间)
               //此变量用于控制电机运行速度.增大此变量数值将
               //降低电机运行速度从而控制机械臂动作速度。

// 定义电机控制用常量
const int enablePin = 10;   // 使能控制引脚
const int enablePin1 = 11;  // 使能控制引脚
// A4988连接Arduino引脚号
const int xdirPin = 6;   // x方向控制引脚
const int xstepPin = 2;  // x步进控制引脚

const int ydirPin = 7;   // y方向控制引脚
const int ystepPin = 3;  // y步进控制引脚

const int zdirPin = 8;   // z方向控制引脚
const int zstepPin = 4;  // z步进控制引脚

const int adirPin = 9;   // a方向控制引脚
const int astepPin = 5;  // a步进控制引脚

//const int moveSteps = 1000;  //运行步数


//存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变)
const int oneMin = 0;
const int oneMax = 180;
const int twoMin = 0;
const int twoMax = 180;
const int threeMin = 0;
const int threeMax = 180;
const int fourMin = 0;
const int fourMax = 180;

AccelStepper stepper1(1, xstepPin, xdirPin);  //建立步进电机对象
AccelStepper stepper2(1, ystepPin, ydirPin);  //建立步进电机对象2
AccelStepper stepper3(1, zstepPin, zdirPin);  //建立步进电机对象3
AccelStepper stepper4(1, astepPin, adirPin);  //建立步进电机对象4

Servo one, two, three, four;  //创建4个servo对象

void setup() {
  Serial.begin(9600);

  pinMode(xstepPin, OUTPUT);  // Arduino控制A4988x步进引脚为输出模式
  pinMode(xdirPin, OUTPUT);   // Arduino控制A4988x方向引脚为输出模式
  pinMode(ystepPin, OUTPUT);  // Arduino控制A4988y步进引脚为输出模式
  pinMode(ydirPin, OUTPUT);   // Arduino控制A4988y方向引脚为输出模式
  pinMode(zstepPin, OUTPUT);  // Arduino控制A4988z步进引脚为输出模式
  pinMode(zdirPin, OUTPUT);   // Arduino控制A4988z方向引脚为输出模式
  pinMode(astepPin, OUTPUT);  // Arduino控制A4988a步进引脚为输出模式
  pinMode(adirPin, OUTPUT);   // Arduino控制A4988a方向引脚为输出模式

  pinMode(enablePin, OUTPUT);  // Arduino控制A4988使能引脚为输出模式
  //digitalWrite(enablePin, LOW);   // 将使能控制引脚设置为低电平从而让
  // 电机驱动板进入工作状态
  pinMode(enablePin1, OUTPUT);  // Arduino控制A4988使能引脚为输出模式
  //digitalWrite(enablePin1, LOW);  // 将使能控制引脚设置为低电平从而让
  // 电机驱动板进入工作状态

  stepper1.setMaxSpeed(1000.0);     // 设置电机最大速度1000
  stepper1.setAcceleration(100.0);  // 设置电机加速度100.0
  stepper2.setMaxSpeed(1000.0);     // 设置电机最大速度300
  stepper2.setAcceleration(100.0);  // 设置电机加速度20.0
  stepper3.setMaxSpeed(1000.0);     // 设置电机最大速度300
  stepper3.setAcceleration(100.0);  // 设置电机加速度20.0
  stepper4.setMaxSpeed(1000.0);     // 设置电机最大速度300
  stepper4.setAcceleration(100.0);  // 设置电机加速度20.0


  one.attach(22);    // one 伺服舵机连接引脚11 舵机代号'b'
  delay(200);        // 稳定性等待
  two.attach(23);    // two 伺服舵机连接引脚10 舵机代号'r'
  delay(200);        // 稳定性等待
  three.attach(48);  // thre 伺服舵机连接引脚9  舵机代号'f'
  delay(200);        // 稳定性等待
  four.attach(49);   // four 伺服舵机连接引脚6  舵机代号'c'
  delay(200);        // 稳定性等待

  one.write(90);
  delay(10);
  three.write(90);
  delay(10);
  two.write(90);
  delay(10);
  four.write(90);
  delay(10);
}

void loop() {
  if (Serial.available()) {  // 检查串口缓存是否有数据等待传输
    cmd = Serial.read();     // 获取电机指令中指令信息
    Serial.print(F("cmd = "));
    Serial.print(cmd);
    Serial.print(F(" , "));

    data = Serial.parseInt();  // 获取电机指令中参数信息
    Serial.print(F("data = "));
    Serial.print(data);
    Serial.println(F(""));

    runUsrCmd();
  }

  stepper1.run();  // 1号电机运行
  stepper2.run();  // 2号电机运行
  stepper3.run();  // 3号电机运行
  stepper4.run();  // 4号电机运行
}

void runUsrCmd() {
  switch (cmd) {
    case 'o':  //利用currentPosition获取当前电机输出轴位置并通过串口监视器输出该信息

      Serial.print(F("stepper1 Position: "));
      Serial.println(stepper1.currentPosition());
      Serial.print(F("stepper2 Position: "));
      Serial.println(stepper2.currentPosition());
      Serial.print(F("stepper3 Position: "));
      Serial.println(stepper3.currentPosition());
      Serial.print(F("stepper4 Position: "));
      Serial.println(stepper4.currentPosition());
      Serial.print(F("Current Running Motor: "));

      if (stepperNum == 1 || stepperNum == 2 || stepperNum == 3 || stepperNum == 4) {
        Serial.print(F("Motor# "));
        Serial.println(stepperNum);
      } else if (stepperNum == 0) {
        Serial.println(F("4 Motors"));
      }
      break;

    case 'v':  //利用moveTo函数使电机运行到用户指定坐标位置，moveTo函数不会“block”程序。

      if (stepperNum == 1) {
        Serial.print(F("Motor1 'moveTo' "));
        Serial.println(data);
        stepper1.moveTo(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Motor2 'moveTo' "));
        Serial.println(data);
        stepper2.moveTo(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Motor3 'moveTo' "));
        Serial.println(data);
        stepper3.moveTo(data);
      } else if (stepperNum == 4) {
        Serial.print(F("Motor4 'moveTo' "));
        Serial.println(data);
        stepper4.moveTo(data);
      } else if (stepperNum == 0) {
        Serial.print(F("4 Motors 'moveTo' "));
        Serial.println(data);
        stepper1.moveTo(data);
        stepper2.moveTo(data);
        stepper3.moveTo(data);
        stepper4.moveTo(data);
      }
      break;

    case 'm':  // 利用move函数使电机运行相应步数。move函数不会“block”程序。

      if (stepperNum == 1) {
        Serial.print(F("Motor1 'move'  "));
        Serial.println(data);
        stepper1.move(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Motor2 'move'  "));
        Serial.println(data);
        stepper2.move(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Motor3 'move'  "));
        Serial.println(data);
        stepper3.move(data);
      } else if (stepperNum == 4) {
        Serial.print(F("Motor4 'move'  "));
        Serial.println(data);
        stepper4.move(data);
      } else if (stepperNum == 0) {
        Serial.print(F("4 Motors 'move'  "));
        Serial.println(data);
        stepper1.move(data);
        stepper2.move(data);
        stepper3.move(data);
        stepper4.move(data);
      }
      break;

    case 'r':  //利用runToNewPosition函数让电机运行到用户指定位置值。
      if (stepperNum == 1) {
        Serial.print(F("Motor1 'runToNewPosition' "));
        Serial.println(data);
        stepper1.runToNewPosition(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Motor2 'runToNewPosition' "));
        Serial.println(data);
        stepper2.runToNewPosition(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Motor3 'runToNewPosition' "));
        Serial.println(data);
        stepper3.runToNewPosition(data);
      } else if (stepperNum == 4) {
        Serial.print(F("Motor4 'runToNewPosition' "));
        Serial.println(data);
        stepper4.runToNewPosition(data);
      } else if (stepperNum == 0) {
        Serial.print(F("4 Motors 'runToNewPosition' "));
        Serial.println(data);
        stepper1.runToNewPosition(data);
        stepper2.runToNewPosition(data);
        stepper3.runToNewPosition(data);
        stepper4.runToNewPosition(data);
      }
      break;

    case 's':  //利用setCurrentPosition函数设置当前位置为用户指定位置值。
      if (stepperNum == 1) {
        Serial.print(F("Set stepper1 Current Position to "));
        Serial.println(data);
        stepper1.setCurrentPosition(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Set stepper2 Current Position to "));
        Serial.println(data);
        stepper2.setCurrentPosition(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Set stepper3 Current Position to "));
        Serial.println(data);
        stepper3.setCurrentPosition(data);
      } else if (stepperNum == 0) {
        stepper1.moveTo(data);
        Serial.print(F("Set 4 steppers' Current Position to "));
        Serial.println(data);
        stepper1.setCurrentPosition(data);
        stepper2.setCurrentPosition(data);
        stepper3.setCurrentPosition(data);
        stepper4.setCurrentPosition(data);
      }
      break;

    case 'a':  //利用setAcceleration函数设置加速度
      if (stepperNum == 1) {
        Serial.print(F("Motor1 'setAcceleration' "));
        Serial.println(data);
        stepper1.setAcceleration(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Motor2 'setAcceleration' "));
        Serial.println(data);
        stepper2.setAcceleration(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Motor3 'setAcceleration' "));
        Serial.println(data);
        stepper3.setAcceleration(data);
      } else if (stepperNum == 4) {
        Serial.print(F("Motor4 'setAcceleration' "));
        Serial.println(data);
        stepper4.setAcceleration(data);
      } else if (stepperNum == 0) {
        Serial.print(F("4 Motors 'setAcceleration' "));
        Serial.println(data);
        stepper1.setAcceleration(data);
        stepper2.setAcceleration(data);
        stepper3.setAcceleration(data);
        stepper4.setAcceleration(data);
      }
      break;

    case 'x':  //利用setMaxSpeed函数设置最大速度
      if (stepperNum == 1) {
        Serial.print(F("Motor1 'setMaxSpeed' "));
        Serial.println(data);
        stepper1.setMaxSpeed(data);
      } else if (stepperNum == 2) {
        Serial.print(F("Motor2 'setMaxSpeed' "));
        Serial.println(data);
        stepper2.setMaxSpeed(data);
      } else if (stepperNum == 3) {
        Serial.print(F("Motor3 'setMaxSpeed' "));
        Serial.println(data);
        stepper3.setMaxSpeed(data);
      } else if (stepperNum == 4) {
        Serial.print(F("Motor4 'setMaxSpeed' "));
        Serial.println(data);
        stepper4.setMaxSpeed(data);
      } else if (stepperNum == 0) {
        Serial.print(F("4 Motors 'setMaxSpeed' "));
        Serial.println(data);
        stepper1.setMaxSpeed(data);
        stepper2.setMaxSpeed(data);
        stepper3.setMaxSpeed(data);
        stepper4.setMaxSpeed(data);
      }
      break;

    case 'd':  //用户通过此指令可指定哪一个电机进行工作
      if (data == 1 || data == 2 || data == 3 || data == 4) {
        stepperNum = data;
        Serial.print(F("Running Motor "));
        Serial.println(stepperNum);
      } else if (data == 0) {
        stepperNum = data;
        Serial.println(F("Running 4 Motors "));
      } else {
        Serial.print(F("Motor Number Wrong."));
      }
      break;
      //////////////////////////电机////////////////
    case 'h':
      servoCmd(cmd, data, DSD);  // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）
      break;
    case 'j':
      servoCmd(cmd, data, DSD);  // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）
      break;
    case 'k':
      servoCmd(cmd, data, DSD);  // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）
      break;
    case 'l':
      servoCmd(cmd, data, DSD);  // 机械臂舵机运行函数（参数：舵机名，目标角度，延迟/速度）

      break;
    case 'p':  // 输出舵机状态信息
      reportStatus();
      break;

    case 'i':
      armIniPos();
      break;
    case 'e':
      digitalWrite(enablePin, LOW);   // 将使能控制引脚设置为低电平从而让
                                  // 电机驱动板进入工作状态
      digitalWrite(enablePin1, LOW);   // 将使能控制引脚设置为低电平从而让
                                // 电机驱动板进入工作状态
      break;
      case 'n':
      digitalWrite(enablePin, high);   // 将使能控制引脚设置为高电平从而让
      digitalWrite(enablePin1, high);   // 将使能控制引脚设置为高电平从而让
      break;
    default:  // 未知指令
      Serial.println(F("Unknown Command"));
  }
}



void servoCmd(char servoName, int toPos, int servoDelay) {
  Servo servo2go;  //创建servo对象

  //串口监视器输出接收指令信息
  Serial.println("");
  Serial.print("+Command: Servo ");
  Serial.print(servoName);
  Serial.print(" to ");
  Serial.print(toPos);
  Serial.print(" at servoDelay value ");
  Serial.print(servoDelay);
  Serial.println(".");
  Serial.println("");

  int fromPos;  //建立变量，存储电机起始运动角度值

  switch (servoName) {
    case 'h':
      if (toPos >= oneMin && toPos <= oneMax) {
        servo2go = one;
        fromPos = one.read();  // 获取当前电机角度值用于“电机运动起始角度值”
        break;
      } else {
        Serial.println("+Warning: one Servo Value Out Of Limit!");
        return;
      }

    case 'j':
      if (toPos >= twoMin && toPos <= twoMax) {
        servo2go = two;
        fromPos = two.read();  // 获取当前电机角度值用于“电机运动起始角度值”
        break;
      } else {
        Serial.println("+Warning: two Servo Value Out Of Limit!");
        return;
      }

    case 'k':
      if (toPos >= threeMin && toPos <= threeMax) {
        servo2go = three;
        fromPos = three.read();  // 获取当前电机角度值用于“电机运动起始角度值”
        break;
      } else {
        Serial.println("+Warning: three Servo Value Out Of Limit!");
        return;
      }

    case 'l':

      if (toPos >= fourMin && toPos <= fourMax) {
        servo2go = four;
        fromPos = four.read();  // 获取当前电机角度值用于“电机运动起始角度值”
        break;
      } else {
        Serial.println("+Warning: four Servo Value Out Of Limit!");
        return;
      }
  }

  //指挥电机运行
  if (fromPos <= toPos) {  //如果“起始角度值”小于“目标角度值”
    for (int i = fromPos; i <= toPos; i++) {
      servo2go.write(i);
      delay(servoDelay);
    }
  } else {  //否则“起始角度值”大于“目标角度值”
    for (int i = fromPos; i >= toPos; i--) {
      servo2go.write(i);
      delay(servoDelay);
    }
  }
}

void reportStatus() {  //舵机状态信息
  Serial.println("");
  Serial.println("");
  Serial.println("+ Robot-Arm Status Report +");
  Serial.print("four Position: ");
  Serial.println(four.read());
  Serial.print("one Position: ");
  Serial.println(one.read());
  Serial.print("two  Arm Position:");
  Serial.println(two.read());
  Serial.print("three Arm Position:");
  Serial.println(three.read());
  Serial.println("++++++++++++++++++++++++++");
  Serial.println("");
}

void armIniPos() {
  Serial.println("+Command: Restore Initial Position.");
  int robotIniPosArray[4][3] = {
    { 'one', 90, DSD },
    { 'two', 90, DSD },
    { 'three', 90, DSD },
    { 'four', 90, DSD }
  };

  for (int i = 0; i < 4; i++) {
    servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);
  }
}